function [data_q, T] = rotate(data, theta, t)
    theta = -theta/180*pi;
    T = [
        cos(theta), sin(theta), 0, t(1);
       -sin(theta), cos(theta), 0, t(2);
        0,          0,          1, t(3);
        0,          0,          0, 1;
        ];
    rows = size(data, 2);
    rows_one = ones(1, rows);
    data = [data; rows_one];
    data_q = T * data;
    data_q = data_q(1:3,:);